#include <crv.h>
#include <crvBezier.h>
#include <crvTables.h>
#include <crvSnap.h>
#include <crvMath.h>
#include <gmi_analytic.h>
#include <gmi_null.h>
#include <apfMDS.h>
#include <apfMesh2.h>
#include <apf.h>
#include <apfShape.h>
#include <PCU.h>
#include <lionPrint.h>
#include <mth.h>
#include <mth_def.h>
#include <pcu_util.h>
#include <ostream>
/* This file contains miscellaneous tests relating to ordering, math
 * and transformation matrices
 */

static apf::Vector3 points3D[4] =
{apf::Vector3(0,0,0),
    apf::Vector3(1,0,0),
    apf::Vector3(0,1,0),
    apf::Vector3(0,0,1)};

apf::Mesh2* createMesh2D()
{
  gmi_model* model = gmi_load(".null");
  apf::Mesh2* m = apf::makeEmptyMdsMesh(model, 2, false);

  apf::buildOneElement(m,0,apf::Mesh::TRIANGLE,points3D);
  apf::deriveMdsModel(m);

  m->acceptChanges();
  m->verify();
  return m;
}

apf::Mesh2* createMesh3D()
{
  gmi_model* model = gmi_load(".null");
  apf::Mesh2* m = apf::makeEmptyMdsMesh(model, 3, false);

  apf::buildOneElement(m,0,apf::Mesh::TET,points3D);
  apf::deriveMdsModel(m);

  m->acceptChanges();
  m->verify();
  return m;
}

void testNodeIndexing(){
  for(int P = 1; P <= 10; ++P)
    for(int i = 0; i <= P; ++i)
      for(int j = 0; j <= P-i; ++j)
        PCU_ALWAYS_ASSERT(crv::computeTriNodeIndex(P,i,j)
  == crv::getTriNodeIndex(P,i,j));

  for(int P = 1; P <= 4; ++P)
    for(int i = 0; i <= P; ++i)
      for(int j = 0; j <= P-i; ++j)
        for(int k = 0; k <= P-i-j; ++k)
          PCU_ALWAYS_ASSERT(crv::computeTetNodeIndex(P,i,j,k)
  == crv::getTetNodeIndex(P,i,j,k));
}

static double const a_data[35][35] = {{1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0.4759624886330922,0.0008234344110729,0,0,0.3882815810042113,0.1187824065033745,0.0161500894482495,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0.0625,0.0625,0,0,0.25,0.375,0.25,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0.0008234344110728,0.4759624886330922,0,0,0.0161500894482495,0.1187824065033744,0.3882815810042112,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0.4759624886330922,0.0008234344110729,0,0,0,-1e-16,0.3882815810042113,0.1187824065033745,0.0161500894482495,0,0,0,0,0,0,0,0,0,0,0,0,0,-1e-16,0,0,0,0,0,0,0,0,0,0,0},
    {0,0.0625,0.0625,0,0,0,0,0.25,0.375,0.25,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0.0008234344110729,0.4759624886330922,0,0,0,0,0.0161500894482495,0.1187824065033745,0.3882815810042113,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0.0008234344110728,0,0.4759624886330922,0,0,0,0,0,0,0,0.3882815810042112,0.1187824065033744,0.0161500894482495,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0.0625,0,0.0625,0,0,0,0,0,0,0,0.25,0.375,0.25,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0.4759624886330922,0,0.0008234344110729,0,0,0,0,0,0,0,0.0161500894482495,0.1187824065033745,0.3882815810042113,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0.4759624886330922,0,0,0.0008234344110729,0,0,0,0,0,0,0,0,0,0.3882815810042113,0.1187824065033745,0.0161500894482495,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0.0625,0,0,0.0625,0,0,0,0,0,0,0,0,0,0.25,0.375,0.25,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0.0008234344110728,0,0,0.4759624886330922,0,0,0,0,0,0,0,0,0,0.0161500894482495,0.1187824065033744,0.3882815810042112,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0.4759624886330922,0,0.0008234344110729,0,0,-1e-16,0,0,0,0,0,0,0,0,0,0.3882815810042113,0.1187824065033745,0.0161500894482495,0,0,0,0,0,0,0,-1e-16,0,0,0,0,0,0,0,0},
    {0,0.0625,0,0.0625,0,0,0,0,0,0,0,0,0,0,0,0,0.25,0.375,0.25,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0.0008234344110729,0,0.4759624886330922,0,0,0,0,0,0,0,0,0,0,0,0,0.0161500894482495,0.1187824065033745,0.3882815810042113,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0,0.4759624886330922,0.0008234344110729,0,0,0,0,0,0,-1e-16,0,0,0,0,0,0,0,0,0.3882815810042113,0.1187824065033745,0.0161500894482495,0,0,0,0,0,0,0,0,0,0,-1e-16,0,0},
    {0,0,0.0625,0.0625,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.25,0.375,0.25,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0,0.0008234344110729,0.4759624886330922,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.0161500894482495,0.1187824065033745,0.3882815810042113,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0.09710323671528701,0.0023806134614917,0.0023806134614917,0,0.1536942048287659,0.0912247214807357,0.0240649565400464,0.0095224538459668,0.0142836807689502,0.0095224538459668,0.0240649565400464,0.0912247214807357,0.1536942048287659,0,0,0,0,0,0,0,0,0,0.1824494429614713,0.0721948696201392,0.0721948696201392,0,0,0,0,0,0,0,0,0,0},
    {0.0023806134614917,0.09710323671528701,0.0023806134614917,0,0.0240649565400464,0.0912247214807357,0.1536942048287659,0.1536942048287659,0.0912247214807357,0.0240649565400464,0.0095224538459668,0.0142836807689502,0.0095224538459668,0,0,0,0,0,0,0,0,0,0.0721948696201392,0.1824494429614713,0.0721948696201392,0,0,0,0,0,0,0,0,0,0},
    {0.0023806134614917,0.0023806134614917,0.09710323671528701,0,0.0095224538459668,0.0142836807689502,0.0095224538459668,0.0240649565400464,0.0912247214807357,0.1536942048287659,0.1536942048287659,0.0912247214807357,0.0240649565400464,0,0,0,0,0,0,0,0,0,0.0721948696201392,0.0721948696201392,0.1824494429614713,0,0,0,0,0,0,0,0,0,0},
    {0.09710323671528701,0.0023806134614917,0,0.0023806134614917,0.1536942048287659,0.0912247214807357,0.0240649565400464,0,0,0,0,0,0,0.1536942048287659,0.0912247214807357,0.0240649565400464,0.0095224538459668,0.0142836807689502,0.0095224538459668,0,0,0,0,0,0,0.1824494429614713,0.0721948696201392,0.0721948696201392,0,0,0,0,0,0,0},
    {0.0023806134614917,0.09710323671528701,0,0.0023806134614917,0.0240649565400464,0.0912247214807357,0.1536942048287659,0,0,0,0,0,0,0.0095224538459668,0.0142836807689502,0.0095224538459668,0.1536942048287659,0.0912247214807357,0.0240649565400464,0,0,0,0,0,0,0.0721948696201392,0.1824494429614713,0.0721948696201392,0,0,0,0,0,0,0},
    {0.0023806134614917,0.0023806134614917,0,0.09710323671528701,0.0095224538459668,0.0142836807689502,0.0095224538459668,0,0,0,0,0,0,0.0240649565400464,0.0912247214807357,0.1536942048287659,0.0240649565400464,0.0912247214807357,0.1536942048287659,0,0,0,0,0,0,0.0721948696201392,0.0721948696201392,0.1824494429614713,0,0,0,0,0,0,0},
    {0,0.09710323671528701,0.0023806134614917,0.0023806134614917,0,0,0,0.1536942048287659,0.0912247214807357,0.0240649565400464,0,0,0,0,0,0,0.1536942048287659,0.0912247214807357,0.0240649565400464,0.0095224538459668,0.0142836807689502,0.0095224538459668,0,0,0,0,0,0,0.1824494429614713,0.0721948696201392,0.0721948696201392,0,0,0,0},
    {0,0.0023806134614917,0.09710323671528701,0.0023806134614917,0,0,0,0.0240649565400464,0.0912247214807357,0.1536942048287659,0,0,0,0,0,0,0.0095224538459668,0.0142836807689502,0.0095224538459668,0.1536942048287659,0.0912247214807357,0.0240649565400464,0,0,0,0,0,0,0.0721948696201392,0.1824494429614713,0.0721948696201392,0,0,0,0},
    {0,0.0023806134614917,0.0023806134614917,0.09710323671528701,0,0,0,0.0095224538459668,0.0142836807689502,0.0095224538459668,0,0,0,0,0,0,0.0240649565400464,0.0912247214807357,0.1536942048287659,0.0240649565400464,0.0912247214807357,0.1536942048287659,0,0,0,0,0,0,0.0721948696201392,0.0721948696201392,0.1824494429614713,0,0,0,0},
    {0.09710323671528701,0,0.0023806134614917,0.0023806134614917,0,0,0,0,0,0,0.0240649565400464,0.0912247214807357,0.1536942048287659,0.1536942048287659,0.0912247214807357,0.0240649565400464,0,0,0,0.0095224538459668,0.0142836807689502,0.0095224538459668,0,0,0,0,0,0,0,0,0,0.1824494429614713,0.0721948696201392,0.0721948696201392,0},
    {0.0023806134614917,0,0.09710323671528701,0.0023806134614917,0,0,0,0,0,0,0.1536942048287659,0.0912247214807357,0.0240649565400464,0.0095224538459668,0.0142836807689502,0.0095224538459668,0,0,0,0.1536942048287659,0.0912247214807357,0.0240649565400464,0,0,0,0,0,0,0,0,0,0.0721948696201392,0.1824494429614713,0.0721948696201392,0},
    {0.0023806134614917,0,0.0023806134614917,0.09710323671528701,0,0,0,0,0,0,0.0095224538459668,0.0142836807689502,0.0095224538459668,0.0240649565400464,0.0912247214807357,0.1536942048287659,0,0,0,0.0240649565400464,0.0912247214807357,0.1536942048287659,0,0,0,0,0,0,0,0,0,0.0721948696201392,0.0721948696201392,0.1824494429614713,0},
    {0.00390625,0.00390625,0.00390625,0.00390625,0.015625,0.0234375,0.015625,0.015625,0.0234375,0.015625,0.015625,0.0234375,0.015625,0.015625,0.0234375,0.015625,0.015625,0.0234375,0.015625,0.015625,0.0234375,0.015625,0.046875,0.046875,0.046875,0.046875,0.046875,0.046875,0.046875,0.046875,0.046875,0.046875,0.046875,0.046875,0.09375}};

static double const a_invdata[35][35] = {{1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {-1.526804207661546,-0.2500000000000001,0,0,3.375676033412428,-1.28732567375034,0.6884538479994579,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {1.017869471774364,1.017869471774365,0,0,-2.709419920941257,4.383100898333787,-2.709419920941258,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {-0.2499999999999995,-1.526804207661547,0,0,0.6884538479994572,-1.28732567375034,3.375676033412429,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,-1.526804207661547,-0.2500000000000002,0,0,0,0,3.375676033412427,-1.287325673750341,0.6884538479994581,0,0,0,0,0,0,0,0,0,0,0,0,0,2.385415368501797e-15,0,0,0,0,0,0,0,0,0,0,0},
    {0,1.017869471774365,1.017869471774364,0,0,0,0,-2.709419920941257,4.383100898333788,-2.709419920941257,0,0,0,0,0,0,0,0,0,0,0,0,0,-1.914606690679597e-15,0,0,0,0,0,0,0,0,0,0,0},
    {0,-0.25,-1.526804207661547,0,0,0,0,0.6884538479994575,-1.287325673750341,3.375676033412428,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {-0.2499999999999996,0,-1.526804207661547,0,0,0,0,0,0,0,3.375676033412429,-1.28732567375034,0.6884538479994573,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {1.017869471774364,0,1.017869471774365,0,0,0,0,0,0,0,-2.709419920941258,4.383100898333788,-2.709419920941257,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {-1.526804207661547,0,-0.2500000000000002,0,0,0,0,0,0,0,0.6884538479994581,-1.287325673750341,3.375676033412428,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {-1.526804207661546,0,0,-0.2500000000000001,0,0,0,0,0,0,0,0,0,3.375676033412428,-1.28732567375034,0.6884538479994579,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {1.017869471774364,0,0,1.017869471774365,0,0,0,0,0,0,0,0,0,-2.709419920941257,4.383100898333787,-2.709419920941258,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {-0.2499999999999995,0,0,-1.526804207661547,0,0,0,0,0,0,0,0,0,0.6884538479994572,-1.28732567375034,3.375676033412429,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,-1.526804207661547,0,-0.2500000000000002,0,0,0,0,0,0,0,0,0,0,0,0,3.375676033412427,-1.287325673750341,0.6884538479994581,0,0,0,0,0,0,0,2.385415368501797e-15,0,0,0,0,0,0,0,0},
    {0,1.017869471774365,0,1.017869471774364,0,0,0,0,0,0,0,0,0,0,0,0,-2.709419920941257,4.383100898333788,-2.709419920941257,0,0,0,0,0,0,0,-1.914606690679597e-15,0,0,0,0,0,0,0,0},
    {0,-0.25,0,-1.526804207661547,0,0,0,0,0,0,0,0,0,0,0,0,0.6884538479994575,-1.287325673750341,3.375676033412428,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0,-1.526804207661547,-0.2500000000000002,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3.375676033412427,-1.287325673750341,0.6884538479994581,0,0,0,0,0,0,0,0,0,0,2.385415368501797e-15,0,0},
    {0,0,1.017869471774365,1.017869471774364,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-2.709419920941257,4.383100898333788,-2.709419920941257,0,0,0,0,0,0,0,0,0,0,-1.914606690679597e-15,0,0},
    {0,0,-0.25,-1.526804207661547,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.6884538479994575,-1.287325673750341,3.375676033412428,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {1.473866405971785,-0.4873245458152488,-0.4873245458152483,0,-2.157170326583087,-0.789537182578665,1.002268609355066,0.4569922360188253,0.4160673296503343,0.456992236018825,1.002268609355065,-0.7895371825786652,-2.157170326583087,0,0,0,0,0,0,0,0,0,7.066481928037422,-2.00343662222666,-2.00343662222666,0,0,0,0,0,0,0,0,0,0},
    {-0.4873245458152486,1.473866405971785,-0.4873245458152484,0,1.002268609355066,-0.7895371825786656,-2.157170326583088,-2.157170326583087,-0.7895371825786647,1.002268609355065,0.4569922360188253,0.4160673296503344,0.4569922360188249,0,0,0,0,0,0,0,0,0,-2.00343662222666,7.066481928037422,-2.00343662222666,0,0,0,0,0,0,0,0,0,0},
    {-0.4873245458152489,-0.4873245458152486,1.473866405971784,0,0.4569922360188252,0.4160673296503342,0.4569922360188253,1.002268609355065,-0.7895371825786655,-2.157170326583087,-2.157170326583087,-0.7895371825786657,1.002268609355066,0,0,0,0,0,0,0,0,0,-2.00343662222666,-2.003436622226659,7.066481928037422,0,0,0,0,0,0,0,0,0,0},
    {1.473866405971786,-0.4873245458152485,0,-0.4873245458152488,-2.157170326583088,-0.7895371825786649,1.002268609355066,0,0,0,0,0,0,-2.157170326583087,-0.7895371825786649,1.002268609355066,0.456992236018825,0.4160673296503342,0.4569922360188252,0,0,0,0,0,0,7.066481928037422,-2.00343662222666,-2.00343662222666,0,0,0,0,0,0,0},
    {-0.4873245458152486,1.473866405971785,0,-0.4873245458152487,1.002268609355066,-0.7895371825786656,-2.157170326583088,0,0,0,0,0,0,0.456992236018825,0.4160673296503344,0.4569922360188255,-2.157170326583087,-0.789537182578665,1.002268609355065,0,0,0,0,0,0,-2.00343662222666,7.066481928037422,-2.003436622226661,0,0,0,0,0,0,0},
    {-0.4873245458152488,-0.4873245458152487,0,1.473866405971785,0.4569922360188252,0.4160673296503344,0.4569922360188257,0,0,0,0,0,0,1.002268609355066,-0.7895371825786659,-2.157170326583088,1.002268609355066,-0.7895371825786657,-2.157170326583087,0,0,0,0,0,0,-2.003436622226661,-2.003436622226661,7.066481928037424,0,0,0,0,0,0,0},
    {0,1.473866405971785,-0.4873245458152484,-0.4873245458152483,0,0,0,-2.157170326583087,-0.7895371825786648,1.002268609355065,0,0,0,0,0,0,-2.157170326583087,-0.7895371825786647,1.002268609355065,0.4569922360188252,0.4160673296503343,0.4569922360188252,0,-1.524360512849797e-15,0,0,-1.524360512849797e-15,0,7.066481928037422,-2.00343662222666,-2.00343662222666,0,0,0,0},
    {0,-0.4873245458152486,1.473866405971785,-0.4873245458152485,0,0,0,1.002268609355065,-0.7895371825786658,-2.157170326583087,0,0,0,0,0,0,0.4569922360188253,0.4160673296503344,0.4569922360188254,-2.157170326583087,-0.7895371825786648,1.002268609355065,0,0,0,0,0,0,-2.003436622226661,7.066481928037424,-2.003436622226661,0,-1.524360512849797e-15,0,0},
    {0,-0.4873245458152486,-0.4873245458152487,1.473866405971785,0,0,0,0.4569922360188253,0.4160673296503344,0.4569922360188254,0,0,0,0,0,0,1.002268609355065,-0.7895371825786657,-2.157170326583087,1.002268609355066,-0.7895371825786657,-2.157170326583087,0,0,0,0,0,0,-2.003436622226661,-2.003436622226661,7.066481928037424,0,0,0,0},
    {1.473866405971785,0,-0.4873245458152488,-0.4873245458152486,0,0,0,0,0,0,1.002268609355066,-0.7895371825786652,-2.157170326583087,-2.157170326583087,-0.7895371825786647,1.002268609355066,0,0,0,0.4569922360188252,0.4160673296503343,0.4569922360188252,0,0,0,0,0,0,0,0,0,7.066481928037422,-2.00343662222666,-2.00343662222666,0},
    {-0.4873245458152485,0,1.473866405971785,-0.4873245458152487,0,0,0,0,0,0,-2.157170326583087,-0.7895371825786656,1.002268609355065,0.4569922360188249,0.4160673296503343,0.4569922360188255,0,0,0,-2.157170326583087,-0.7895371825786648,1.002268609355065,0,0,0,0,0,0,0,0,0,-2.003436622226659,7.066481928037422,-2.003436622226661,0},
    {-0.487324545815249,0,-0.4873245458152486,1.473866405971785,0,0,0,0,0,0,0.4569922360188253,0.4160673296503343,0.4569922360188254,1.002268609355066,-0.7895371825786661,-2.157170326583088,0,0,0,1.002268609355066,-0.7895371825786659,-2.157170326583087,0,0,0,0,0,0,0,0,0,-2.003436622226661,-2.00343662222666,7.066481928037424,0},
    {-0.6654926381785982,-0.6654926381785983,-0.6654926381785977,-0.6654926381785985,0.6979094812091967,0.4963403688403295,0.6979094812091966,0.6979094812091965,0.4963403688403295,0.697909481209196,0.6979094812091964,0.4963403688403295,0.6979094812091965,0.6979094812091965,0.4963403688403297,0.6979094812091967,0.6979094812091963,0.4963403688403293,0.6979094812091965,0.697909481209196,0.4963403688403293,0.6979094812091966,-1.529804341792051,-1.529804341792051,-1.529804341792051,-1.529804341792051,-1.52980434179205,-1.529804341792051,-1.529804341792051,-1.529804341792051,-1.529804341792051,-1.529804341792051,-1.52980434179205,-1.529804341792051,10.66666666666667}};

void testMatrixInverse(){


  mth::Matrix<double> A(35,35);
  for (int i = 0; i < 35; ++i)
    for (int j = 0; j < 35; ++j)
      A(i,j) = a_data[i][j];
  mth::Matrix<double> AiQR(35,35), AiPLU(35,35);

  crv::invertMatrixWithPLU(35,A,AiPLU);
  crv::invertMatrixWithQR(35,A,AiQR);

  mth::Matrix<double> eyeQR(35,35), eyePLU(35,35);
  mth::multiply(A,AiQR,eyeQR);
  mth::multiply(A,AiPLU,eyePLU);

  for (int i = 0; i < 35; ++i)
    for (int j = 0; j < 35; ++j){
      PCU_ALWAYS_ASSERT(fabs(AiQR(i,j) - a_invdata[i][j]) < 1e-14);
      PCU_ALWAYS_ASSERT(fabs(eyeQR(i,j) - (i == j)) < 1e-14);
      PCU_ALWAYS_ASSERT(fabs(AiPLU(i,j) - a_invdata[i][j]) < 1e-14);
      PCU_ALWAYS_ASSERT(fabs(eyePLU(i,j) - (i == j)) < 1e-14);
    }

}

int main(int argc, char** argv)
{
  MPI_Init(&argc,&argv);
  PCU_Comm_Init();
  lion_set_verbosity(1);
  testNodeIndexing();
  testMatrixInverse();
  PCU_Comm_Free();
  MPI_Finalize();
}

